using System;
 using Microsoft.SPOT;
 using Microsoft.SPOT.Hardware;

 
 namespace MLRobotic.Asserv.Drivers
 {
 
     /**********************************************************************
     // Routine : LS7366 class
     //---------------------------------------------------------------------
     // Date : 20110516
     // Name : Wim Cranen
     //---------------------------------------------------------------------
     // Version : v4.0
     //---------------------------------------------------------------------
     // History : v1.0 
     //         : v2.0 Clear and config
     //         : v3.0 Class extended with read CNTR function
     //         : v4.0 Made class smaller and more compact
     //         : v5.0 MLrobotic adaptation
     **********************************************************************/
 
     /*******************************************************************
     // LS7366 REGISTERS
     //------------------------------------------------------------------
     // MDR0 : MoDe Register 0 (8 bit read/write)
     //        Upon power up, MDR0 is set to zero
     //        B1 B0: count mode 1
     //            00 = non quadrature mode (A = clock, B = direction)
     //            01 = x1 quadrature mode (one count per quad. cycle)
     //            10 = x2 quadrature mode (two counts per quad. cycle)
     //            11 = x4 quadrature mode (four counts per quad. cycle)
     //        B3 B2: count mode 2
     //            00 = free running count mode
     //            01 = single-cycle count mode (disabled on carry or
     //                 borrow, resumed on reset)
     //            10 = range limit count mode (up and down between DTR
     //                 and zero, freezes at limit and resumes on 
     //                 direction change
     //            11 = modulo-n count, where n = DTR in both directions
     //        B5 B4: index function
     //            00 = 
     //            01 =
     //            10 =
     //            11 =
     //        B6:  0 = asynchronous index
     //             1 = synchronous index (overriden in non-quad. mode)
     //        B7:  0 = filter clock division factor = 1
     //             1 = filter clock division factor = 2
     //------------------------------------------------------------------
     // MDR1 : MoDe Register 1 (8 bit read/write) as appendix to MDR0
     //        Upon power up, MDR0 is set to zero
     //        B1 B0: count mode 1
     //            00 = 4-byte counter mode
     //            01 = 3-byte counter mode
     //            10 = 2-byte counter mode
     //            11 = 1-byte counter mode
     //        B2:  0 = enable counting
     //             1 = disable counting
     //        B3:  not used
     //        B4:  0 = NOP
     //             1 = FLAG on IDX (B4 of STR) \
     //        B5:  0 = NOP                      |
     //             1 = FLAG on CMP (B5 of STR)  |  NOTE: applicable on
     //        B6:  0 = NOP                       > both LFLAG and DFLAG
     //             1 = FLAG on BW (B6 of STR)   |
     //        B7:  0 = NOP                      |
     //             1 = FLAG on CY (B7 of STR)  /
     //------------------------------------------------------------------
     // DTR  : DaTa Register input (8, 16, 24 or 32 bits)
     //        Can be written directly from MOSI
     //------------------------------------------------------------------
     // CNTR : CouNTer Register (8, 16, 24 or 32 bits)
     //        This register counts the up/down pulses from the quad.
     //        clock applied at the A and B inputs or alternatively
     //        from the A input in non-quad. mode
     //------------------------------------------------------------------
     // OTR  : OuTput Register (8, 16, 24 or 32 bits)
     //        Shadow register from CNTR to read the counter value
     //        during counting. Can be read directly from MOSI while
     //        the counter still counts
     //------------------------------------------------------------------
     // STR  : STatus Register (8 bit read)
     //        B0: S   = signbit (0 neg, 1 pos)
     //        B1: U/D = up/down indicator (0 down, 1 up)
     //        B2: PLS = power loss indicator latch; set upon powerup
     //        B3: CEN = count enable status (0 disabled, 1 enabled)
     //        B4: IDX = index latch
     //        B5: CMP = compare (CNTR = DTR) latch
     //        B6: BW  = borrow (CNTR underlow) latch
     //        B7: CY  = carry (CNTR overflow) latch
     //------------------------------------------------------------------
     // IR   : Instruction Register (8 bit write)
     //        B2 B1 B0 : XXX (don't care)
     //        B5 B4 B3 : 000 = select none
     //                   001 = select MDR0
     //                   010 = select MDR1
     //                   011 = select DTR
     //                   100 = select CNTR
     //                   101 = select OTR
     //                   110 = select STR
     //                   111 = select none
     //           B7 B6 :  00 = CLR register
     //                    01 = RD register
     //                    10 = WR register
     //                    11 = LOAD register
     /*******************************************************************
     // Number of Bytes | OP Code | Register |  Operation
     //-----------------+---------+----------+---------------------------
     //                 |         | MDR0     | Clear MDR0 to zero
     //                 |         | MDR1     | Clear MDR1 to zero
     //       1         |  CLR    | DTR      | None
     //                 |         | CNTR     | Clear CNTR to zero
     //                 |         | OTR      | None
     //                 |         | STR      | Clear STR to zero
     //-----------------+---------+----------+---------------------------             
     //                 |         | MDR0     | Output MDR0 on MISO
     //                 |         | MDR1     | Output MDR1 on MISO
     //    2 to 5       |  RD     | DTR      | None
     //                 |         | CNTR     | Transfer CNTR to OTR, 
     //                 |         |          | then output OTR on MISO
     //                 |         | OTR      | Output OTR on MISO
     //                 |         | STR      | Output STR on MISO
     //-----------------+---------+----------+---------------------------
     //                 |         | MDR0     | Write at MOSI into MDR0
     //                 |         | MDR1     | Write at MOSI into MDR1
     //    2 to 5       |  WR     | DTR      | Write at MOSI into DTR
     //                 |         | CNTR     | None
     //                 |         | OTR      | None
     //                 |         | STR      | None
     //-----------------+---------+----------+---------------------------
     //                 |         | MDR0     | None
     //                 |         | MDR1     | None
     //       1         |  LOAD   | DTR      | None
     //                 |         | CNTR     | Transfer DTR to CNTR
     //                 |         | OTR      | Transfer CNTR to OTR
     *******************************************************************/
 
 
     #region Registers, commands and modes
 
     // LS7366 Commands
     public enum Commands : byte
     {
         LS7366_CLEAR = 0x00, // clear register
         LS7366_READ  = 0x40, // read register
         LS7366_WRITE = 0x80, // ,write register
         LS7366_LOAD  = 0xC0, // load register
     }
 
     // LS7366 Registers
     public enum Registers : byte
     {
         LS7366_MDR0 = 0x08, // select MDR0
         LS7366_MDR1 = 0x10, // select MDR1
         LS7366_DTR  = 0x18, // select DTR
         LS7366_CNTR = 0x20, // select CNTR
         LS7366_OTR  = 0x28, // select OTR
         LS7366_STR  = 0x30, // select STR
     }
 
     // LS7366 MDR0 Counter modes
     public enum MDR0Mode : byte
     {
         LS7366_MDR0_QUAD0 = 0x00, // none quadrature mode
         LS7366_MDR0_QUAD1 = 0x01, // quadrature x1 mode
         LS7366_MDR0_QUAD2 = 0x02, // quadrature x2 mode
         LS7366_MDR0_QUAD4 = 0x03, // quadrature x4 mode
         LS7366_MDR0_FREER = 0x00, // free run mode
         LS7366_MDR0_SICYC = 0x04, // single cycle count mode
         LS7366_MDR0_RANGE = 0x08, // range limit count mode (0-DTR-0)
                                   // counting freezes at limits but 
                                   // resumes on direction reverse
         LS7366_MDR0_MODTR = 0x0C, // modulo-n count (n=DTR both dirs)
         LS7366_MDR0_DIDX  = 0x00, // disable index
         LS7366_MDR0_LDCNT = 0x10, // config IDX as load DTR to CNTR
         LS7366_MDR0_RECNT = 0x20, // config IDX as reset CNTR (=0)
         LS7366_MDR0_LDOTR = 0x30, // config IDX as load CNTR to OTR  
         LS7366_MDR0_ASIDX = 0x00, // asynchronous index
         LS7366_MDR0_SYINX = 0x40, // synchronous IDX (if !NQUAD)
         LS7366_MDR0_FFAC1 = 0x00, // filter clock division factor=1
         LS7366_MDR0_FFAC2 = 0x80, // filter clock division factor=2
         LS7366_MDR0_NOFLA = 0x00, // no flags
     }
 
     // LS7366 MDR1 counter modes
     public enum MDR1Mode : byte
     {
         LS7366_MDR1_4BYTE = 0x00, // 4 byte counter mode
         LS7366_MDR1_3BYTE = 0x01, // 3 byte counter mode
         LS7366_MDR1_2BYTE = 0x02, // 2 byte counter mode
         LS7366_MDR1_1BYTE = 0x03, // 1 byte counter mode
         LS7366_MDR1_ENCNT = 0x00, // enable counting
         LS7366_MDR1_DICNT = 0x04, // disable counting
         LS7366_MDR1_FLIDX = 0x20, // FLAG on IDX (index)
         LS7366_MDR1_FLCMP = 0x40, // FLAG on CMP (compare)
         LS7366_MDR1_FLCY  = 0x80, // FLAG on CY (carry)
     }
     #endregion Registers, commands and modes
 
     class LS7366
     {
         #region Definitions and vars
         private SPI.Configuration spiConfig;
         private static SPI spi;
  
         // For debug purposes
         bool b_Debug = false;
         #endregion Definitions and vars
 
         #region Init SPI
         // Initialize SPI configuration
         public LS7366(SPI.Configuration spiConfig)
         {
             this.spiConfig = spiConfig;
             if(spi == null)
                 spi = new SPI(spiConfig);
         }
 
         // Connecte SPI configuration to physical I/O
         public LS7366(Cpu.Pin pin, SPI.SPI_module module)
             : this(new SPI.Configuration(pin,         // ChipSelect_Port
                                          false,     // ChipSelect_ActiveState
                                          0,         // ChipSelect_SetupTime
                                          0,         // ChipSelect_HoldTime
                                          false,     // Clock_IdleState
                                          true,         // Clock_Edge
                                          4000,         // Clock_Rate in KHz.
                                          module)    // SPI_module
                                          )
         {
             Init();          
         }
         #endregion Init SPI
 
         #region Buffers
         // Read/write buffers
         byte[] LS7366_1B_wr = new byte[1]; // write one byte
         byte[] LS7366_2B_wr = new byte[2]; // write two bytes
         byte[] LS7366_3B_wr = new byte[5]; // write three bytes
         byte[] LS7366_2B_rd = new byte[2]; // read two bytes
         byte[] LS7366_4B_rd = new byte[4]; // read four bytes
         byte[] LS7366_5B_rd = new byte[5]; // read four bytes
         #endregion Buffers

         private void Init()
         {
             ClearRegister();

             #region Configure MDR0 and MDR1 registers
             // Configure MDR0 register
             byte command = (byte)MDR0Mode.LS7366_MDR0_QUAD4   // none quadrature mode
                          | (byte)MDR0Mode.LS7366_MDR0_FREER   // modulo-n counting 
                          | (byte)MDR0Mode.LS7366_MDR0_DIDX;    // disable index 
             if (b_Debug) { Debug.Print("[MAIN] configure MDR0 " + Asserv.Tools.Convert.OneByte_ToHex(command)); }
             Write2Bytes((byte)Commands.LS7366_WRITE | (byte)Registers.LS7366_MDR0, command);

             // Configure MDR1 register
             command = (byte)MDR1Mode.LS7366_MDR1_4BYTE   // 2 byte counter mode
                     | (byte)MDR1Mode.LS7366_MDR1_ENCNT; // enable counting

             if (b_Debug) { Debug.Print("[MAIN] configure MDR1 " +  Asserv.Tools.Convert.OneByte_ToHex(command)); }
             Write2Bytes((byte)Commands.LS7366_WRITE | (byte)Registers.LS7366_MDR1, command);


             #endregion Configure MDR0 and MDR1 registers


             #region Configure DTR register for DFLAG
             // Configure DTR register to 15 ticks/revolution
             // -- The encoder produces 6000 ticks/revolution
             // -- We want 400 measurements per revolution
             // -- 6000/400 = 15 (subtract 1 because counting starts at ZERO[=binairy count])
             // -- Together with the setting of MDR1Mode.LS7366_MDR1_FLCMP in MDR1
             // -- It enables the DFLAG output (pin 9 on LS7366 )to go LOW while CNTR equals DTR
             //if (b_Debug) { Debug.Print("[MAIN] configure DTR"); }
             //LS7366.ConfigDTR2Bytes(14);
             #endregion Configure DTR register for DFLAG
            
         }

         private void ClearRegister()
         {
            // Clear MDR0 register
            if (b_Debug) { Debug.Print("[MAIN] clear MDR0"); }
            Write1Byte((byte)Commands.LS7366_CLEAR         // clear
                            | (byte)Registers.LS7366_MDR0);       // MDR0

            // Clear MDR1 register
            if (b_Debug) { Debug.Print("[MAIN] clear MDR1"); }
            Write1Byte((byte)Commands.LS7366_CLEAR         // clear
                            | (byte)Registers.LS7366_MDR1);       // MDR1

            // Clear STR register
            if (b_Debug) { Debug.Print("[MAIN] clear STR"); }
            Write1Byte((byte)Commands.LS7366_CLEAR         // clear
                            | (byte)Registers.LS7366_STR);        // STR

            // Clear CNTR
            if (b_Debug) { Debug.Print("[MAIN] clear CNTR"); }
            Write1Byte((byte)Commands.LS7366_CLEAR         // clear
                            | (byte)Registers.LS7366_CNTR);       // CNTR

            // Clear ORT (write CNTR into OTR)
            if (b_Debug) { Debug.Print("[MAIN] load CNTR -> OTR register"); }
            Write1Byte((byte)Commands.LS7366_LOAD          // load
                            | (byte)Registers.LS7366_OTR);        // OTR

        }
         
         // Write one byte to register
         public void Write1Byte(byte reg)
         {
             LS7366_1B_wr[0] = reg;
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_1B_wr[0]);
                 Debug.Print("[1 BYTE WRITE] command is     : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_1B_wr[0]);
                 Debug.Print("[1 BYTE WRITE] command is     : 0b" + bin + "\n");
             }
             spi.Config = spiConfig;
             spi.Write(LS7366_1B_wr);
         }
 
         // Write two bytes to a register
         public void Write2Bytes(byte reg, byte cmd)
         {
             LS7366_2B_wr[0] = reg;
             LS7366_2B_wr[1] = cmd;
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_wr[0]);
                 Debug.Print("[2 BYTE WRITE] command [0] is : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_wr[0]);
                 Debug.Print("[2 BYTE WRITE] command is     : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_wr[1]);
                 Debug.Print("[2 BYTE WRITE] data is        : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_wr[1]);
                 Debug.Print("[2 BYTE WRITE] data is        : 0b" + bin + "\n");
             }
             spi.Config = spiConfig;
             spi.Write(LS7366_2B_wr);
         }
       
         // Configure DTR register as 2 bytes unsigned integer (0 - 65535)
         public void ConfigDTR2Bytes(uint TicksPerRev)
         {
             LS7366_3B_wr[0] = (byte)Commands.LS7366_WRITE | (byte)Registers.LS7366_DTR;
             LS7366_3B_wr[1] = (byte)(TicksPerRev / 256);
             LS7366_3B_wr[2] = (byte)(TicksPerRev % 256);
             if (b_Debug) 
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_3B_wr[0]);
                 Debug.Print("[DTR  CONFIG] command is     : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_3B_wr[0]);
                 Debug.Print("[DTR  CONFIG] command is     : 0b" + bin);
                 Debug.Print("[DTR  CONFIG] data input is  : " + TicksPerRev.ToString() + " as decimal.");
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_3B_wr[1]);
                 Debug.Print("[DTR  CONFIG] data [1] is    : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_3B_wr[1]);
                 Debug.Print("[DTR  CONFIG] data [1] is    : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_3B_wr[2]);
                 Debug.Print("[DTR  CONFIG] data [2] is    : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_3B_wr[2]);
                 Debug.Print("[DTR  CONFIG] data [2] is    : 0b" + bin + "\n");
             }
             spi.Config = spiConfig;
             spi.Write(LS7366_3B_wr);
         }
 
         // Return one byte from a register
         public byte Return1Byte(byte reg)
         {
             byte b;
             LS7366_1B_wr[0] = reg;
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_1B_wr[0]);
                 Debug.Print("[RETURN 1B] command is  : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_1B_wr[0]);
                 Debug.Print("[RETURN 1B] command is  : 0b" + bin);
             }
             spi.Config = spiConfig;
             spi.WriteRead(LS7366_1B_wr, LS7366_2B_rd);
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_rd[0]);
                 Debug.Print("[RETURN 1B] data[0] is  : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_rd[0]);
                 Debug.Print("[RETURN 1B] Data[0] is  : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_rd[1]);
                 Debug.Print("[RETURN 1B] Data[1] is  : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_rd[1]);
                 Debug.Print("[RETURN 1B] Data[1] is  : 0b" + bin + "\n");
             }
             b = LS7366_2B_rd[1];
             return b;
         }

         // Return integer in 2 byte mode
         public int Return2Bytes(byte reg)
         {
             int result;
             LS7366_1B_wr[0] = reg;
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_1B_wr[0]);
                 Debug.Print("[RETURN 2B] command is : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_1B_wr[0]);
                 Debug.Print("[RETURN 2B] command is  : 0b" + bin);
             }
             spi.Config = spiConfig;
             spi.WriteRead(LS7366_1B_wr, LS7366_4B_rd);
             if (b_Debug)
             {
                 string hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_4B_rd[0]);
                 Debug.Print("[RETURN 2B] data[0] is     : 0x" + hex);
                 string bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_4B_rd[0]);
                 Debug.Print("[RETURN 2B] Data[0] is     : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_4B_rd[1]);
                 Debug.Print("[RETURN 2B] data[1] is     : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_4B_rd[1]);
                 Debug.Print("[RETURN 2B] data[1] is     : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_4B_rd[2]);
                 Debug.Print("[RETURN 2B] data[2] is     : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_4B_rd[2]);
                 Debug.Print("[RETURN 2B] data[2] is     : 0b" + bin);
                 hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_4B_rd[3]);
                 Debug.Print("[RETURN 2B] data[3] is     : 0x" + hex);
                 bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_4B_rd[3]);
                 Debug.Print("[RETURN 2B] data[3] is     : 0b" + bin + "\n");
             }
             result = (LS7366_4B_rd[1] * 256) + LS7366_4B_rd[2];
             if (b_Debug) { Debug.Print("[RETURN 2B] data result is : " + result.ToString() + ""); }
             return result;
         }

         // Return integer in 4 byte mode
         public int Return4Bytes(byte reg)
         {
             int result;
             LS7366_1B_wr[0] = reg;
              
             spi.Config = spiConfig; 
             spi.WriteRead(LS7366_1B_wr, LS7366_5B_rd);

             result = (LS7366_5B_rd[1] * 256 * 256 * 256) + LS7366_5B_rd[2] * 256 * 256 + (LS7366_5B_rd[3] * 256) + LS7366_5B_rd[4];

             return result;
         }

         /// <summary>
         /// Read MDR0 et MDR1 Register for test purpose
         /// Must be a 2b config
         /// </summary>
         public void Read_2b_RegisterTest ()
         {
             string str_hex = "F0";          // For string  Asserv.Tools.Convert purposes
             string str_bin = "10101010";    // For string  Asserv.Tools.Convert purposes
             // Read MDR0 register
            //if (b_Debug) { Debug.Print("[MAIN] read MDR0"); }
            int LS7366_2B_rd = Return2Bytes((byte)Commands.LS7366_READ    // read
                                            | (byte)Registers.LS7366_MDR0); // MDR0
            if (b_Debug) 
            {
                str_hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_rd);
                str_bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_rd);
                Debug.Print("[MDR0 READ main data is      : 0x" + str_hex);
                Debug.Print("[MDR0 READ main data is      : 0b" + str_bin + "\n"); 
            }

            // Read MDR1 register
            //if (b_Debug) { Debug.Print("[MAIN] read MDR1"); }
            LS7366_2B_rd = Return2Bytes((byte)Commands.LS7366_READ    // read
                                            | (byte)Registers.LS7366_MDR1); // MDR1
            if (b_Debug) 
            {
                str_hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_2B_rd);
                str_bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_2B_rd);
                Debug.Print("[MDR1 READ main data is      : 0x" + str_hex);
                Debug.Print("[MDR1 READ main data is      : 0b" + str_bin + "\n"); 
            }

            // Read STR register
            //if (b_Debug) { Debug.Print("[MAIN] read STR"); }
            byte LS7366_1B_rd = Return1Byte((byte)Commands.LS7366_READ    // read
                                            | (byte)Registers.LS7366_STR);  // STR
            if (b_Debug)
            {
                str_hex =  Asserv.Tools.Convert.TwoByte_ToHex(LS7366_1B_rd);
                str_bin =  Asserv.Tools.Convert.TwoByte_ToBinMSBF(LS7366_1B_rd);
            //    Debug.Print("[STR READ main data is       : 0x" + str_hex);
                Debug.Print("[STR READ main data is       : 0b" + str_bin + "\n");
            }
         }

         public int ReadCounter()
         {
             return Return4Bytes((byte)Commands.LS7366_READ      //read
                                              | (byte)Registers.LS7366_CNTR);   //CNTR
         }
     }
 }

 